;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Image class
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(defclass image ()
  ((rows :initarg :rows :reader rows)
   (cols :initarg :cols :reader cols)
   (depth :initarg :depth :reader depth)
   (pixels :initarg :pixels :accessor pixels))
  (:documentation "Class of images."))

;;Method: get-pixel
(defgeneric get-pixel (img i j)
  (:documentation "Returns the value of pixel (i,j) if the given image"))

(defmethod get-pixel ((img image) (i integer) (j integer))
  (nth-value 0 (gethash (list i j) (pixels img))))

;;Method: set-pixel
(defgeneric set-pixel (img i j value)
  (:documentation "Set pixel (i,j) of img to the given value"))

(defmethod set-pixel ((img image) (i integer) (j integer) (value number))
  (setf (gethash (list i j) (pixels img)) value))


;;Function: create-image-from-file
(defun create-image-from-file (filename)
  (with-open-file (in filename)
    (with-standard-io-syntax
      (progn
	(read-line in) ; P2
	(read-line in) ; comment #
	(let* ((n (read in))
	       (m (read in))
	       (depth (read in))
	       (data (make-hash-table :key-type 'list :test #'equalp)))
	  (progn
	    (loop for i from 1 to m do
		 (loop for j from 1 to n do
		      (setf (gethash (list i j) data) (read in))))
	    (make-instance 'image :rows m :cols n :depth depth :pixels data)))))))



(defgeneric locate-color (img color-value)
  (:documentation "Return a list of pixel coordinates that match a given color."))

(defmethod locate-color ((img image) (color-value integer))
  (let ((data (loop for key being the hash-keys of (pixels img) collecting key)))
    (remove-if-not #'(lambda (x) (equalp (gethash x (pixels img)) color-value)) data)))
    
    


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Shift class
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defclass shift ()
  ((vertical :initarg :vertical :accessor vertical)
   (horizontal :initarg :horizontal :accessor horizontal))
  (:documentation "Class for pixel's shifts."))

(defun create-shift (a b)
  (make-instance 'shift :vertical a :horizontal b))

(defgeneric shift-difference (sh1 sh2)
  (:documentation "Compute the componentwise difference between sh1 and sh2."))

(defmethod shift-difference ((sh1 shift) (sh2 shift))
  (create-shift (- (vertical sh1) (vertical sh2)) (- (horizontal sh1) (horizontal sh2))))

(defgeneric shift-sum (sh1 sh2)
  (:documentation "Compute thecomponentwise sum of two shifts"))

(defmethod shift-sum ((sh1 shift) (sh2 shift))
  (create-shift (+ (vertical sh1) (vertical sh2)) (+ (horizontal sh1) (horizontal sh2))))



(defgeneric abs-shift (sh)
  (:documentation "Rotate the shift in the right portion of the plane"))

(defmethod abs-shift ((sh shift))
  (let ((v (vertical sh)) (h (horizontal sh)))
    (create-shift (max (abs v) (abs h)) (min (abs v) (abs h)))))

(defgeneric g-term (sh)
  (:documentation "Compute g on the shift."))

(defmethod g-term ((sh shift))
  (let ((tmp (abs-shift sh)))
    (or (gethash (list (vertical tmp) (horizontal tmp)) *matrice-dei-pesi*)
	(add-g-term tmp))))

(defgeneric add-g-term (sh)
  (:documentation "Add the value of g on shift in the hash table of shifts."))

(defmethod add-g-term ((sh shift))
  (setf (gethash (components sh) *matrice-dei-pesi*) (g (vertical sh) (horizontal sh))))

(defun g (a b)
  (sqrt (+ (* a a) (* b b))))

(defvar *matrice-dei-pesi* (make-hash-table :key-type 'list :value-type 'number :test #'equalp))

(defmethod print-object ((sh shift) stream)
  (format stream "(~A,~A)" (vertical sh) (horizontal sh)))

(defgeneric components (sh)
  (:documentation "Return a list with the two components of the shift sh."))

(defmethod components ((sh shift))
  (list (vertical sh) (horizontal sh)))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Ant class 
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun empty-path ()
  (make-hash-table :key-type 'list :value-type 'shift-with-energy))

(defclass ant () 
  ((path :initform (empty-path)  :accessor path)
   (path-energy :initform 0 :accessor path-energy)) 
  (:documentation  "A buggy!!"))


(defun create-ant ()
  (make-instance 'ant))

(defgeneric get-shift (i j bug)
  (:documentation "Retrive the shift of pixel i j choosen the ant."))

(defgeneric set-shift (i j bug sh)
  (:documentation "Set the shift choosen by anty for pixel i j."))

(defgeneric erase-path (bug)
  (:documentation "Clear the path of bug."))

(defmethod erase-path ((bug ant))
  (setf (path bug) (empty-path)))



;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;Shift with energy
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defclass shift-with-energy (shift)
  ((energy :initform 0 :accessor energy))
  (:documentation "Shift with the given energy."))

(defun create-shift-with-energy (v h)
  (make-instance 'shift-with-energy :vertical v :horizontal h))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Mixed method for ants, images and shifts
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defgeneric build-path (bug img1 img2)
  (:documentation "The ant BUG build a path from image img"))

(defmethod build-path ((bug ant) (img1 image) (img2 image))
  (let ((m (rows img1))
	 (n (cols img1)))
    (loop for i from 1 to m do
	 (loop for j from 1 to n do
	      (setf (gethash (list i j) (path bug)) (choose-hij i j img1 img2 bug))))))
    

(defgeneric choose-hij (i j img1 img2 bug)
  (:documentation "The bug choose the shift-with-energy of pixel i,j for img1 and img2."))


(defmethod choose-hij ((i (eql 1)) (j (eql 1)) (img1 image) (img2 image) (bug ant))
  (let* ((places (locate-color img2 (get-pixel img1 1 1)))
	 (trailer (or (gethash (list 1 1) *pheromone-trails*) (create-empty-pheromone-trail))))
    (loop for element in places do (add-default-trail trailer (create-shift (first element) (second element)) 1))
    (loop for key being the hash-keys of (trail trailer) using (hash-value value) do (setf (gethash key (trail trailer)) (energy-A (get-pixel img1 1 1) img2 (create-shift (first key) (second key)))))
    trailer))


(defun square (x)
  (* x x))

(defgeneric energy-A (x img sh)
  (:documentation "Energy values at pixel (1,1)"))

(defmethod energy-A ((x number) (img image) (sh shift))
  (let ((sh1 (shift-sum sh (create-shift 1 1))))
    (square (- x (get-pixel img (vertical sh1) (horizontal sh1))))))
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Pheromone trail class
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun empty-pheromone-trail ()
  (make-hash-table :key-type 'list :value-type 'number :test #'equalp))


(defclass pheromone-trail ()
  ((trail :initform (empty-pheromone-trail) :accessor trail))
  (:documentation "A trail of pheromone"))

(defun create-empty-pheromone-trail ()
  (make-instance 'pheromone-trail))

(defgeneric add-default-trail (pht sh ph)
  (:documentation "Add to the pheromone-trail pht the shift sh with the given pheromone ph.
Because it is for default pheromone, before adding it checks for the existence of the trail:
In this case it adds nothing."))

(defmethod add-default-trail ((pht pheromone-trail) (sh shift) (ph number))
  (or (gethash (components sh) (trail pht))
      (setf (gethash (components sh) (trail pht)) ph)))

;(defmethod add-default-trail ((pht pheromone-trail) (sh shift) (ph number))
;  (if (not (nth-value 1 (gethash (components sh) pht)))
;      (setf (gethash (components sh) pht) ph)))

(defgeneric pheromone-evaporation (pht rho)
  (:documentation "Method for evaporation of pheromone with a costant rho"))


(defmethod pheromone-evaporation ((pht pheromone-trail) (rho number))
  (maphash #'(lambda (key value) (setf (gethash key (trail pht)) (- value rho))) (trail pht)))




(defgeneric delete-empty-value (pht)
  (:documentation "Delete trails with non positive pheromone"))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Algorithm
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

 
(defvar *pheromone-trails* (make-hash-table :key-type 'list :test #'equalp :value-type 'pheromone-trail))

(defvar *test* (create-image-from-file "test.pgm"))

(defvar *test-shifted* (create-image-from-file "test-shifted.pgm"))

;(loop for key being the hash-keys of (trail *prova*) using (hash-value value) collecting (list key value))



(defun eta (x k)
  (let ((a (abs x)))
    (cond
      ((> a k) 0)
      (t (+ 1 (* (/ 1 (- k)) x))))))